/*
 * DistanceSensorManager - 距离传感器管理类实现
 */
#include "DistanceSensorManager.h"

DistanceSensorManager::DistanceSensorManager(int triggerPin, int echoPin, int maxDistance) :
  _triggerPin(triggerPin),
  _echoPin(echoPin),
  _maxDistance(maxDistance),
  _sensor(triggerPin, echoPin, maxDistance),
  _lastDistance(0),
  _status(DISTANCE_SENSOR_IDLE),
  _measureInterval(1000),  // 默认100ms测量一次
  _lastMeasureTime(0),
  _isEnabled(true)
{
}

void DistanceSensorManager::begin() {
  Serial.println("初始化距离传感器...");
  pinMode(_triggerPin, OUTPUT);
  pinMode(_echoPin, INPUT);
  _status = DISTANCE_SENSOR_READY;
  Serial.println("距离传感器初始化完成");
}

void DistanceSensorManager::loop() {
  if (!_isEnabled || _status != DISTANCE_SENSOR_READY) {
    return;
  }
  
  // 定时测量距离
  unsigned long currentTime = millis();
  if (currentTime - _lastMeasureTime >= _measureInterval) {
    _lastMeasureTime = currentTime;
    double distance = measureDistance();
    _lastDistance = distance;
    if (_onDistanceChanged) {
      _onDistanceChanged(distance);
    }
  }
}

double DistanceSensorManager::getDistance() const {
  return _lastDistance;
}

double DistanceSensorManager::measureDistance() {
  if (_status != DISTANCE_SENSOR_READY) {
    return -1;
  }
  
  double distance = _sensor.centimeters();
  
  // 检查测量结果是否有效
  if (distance < 0 || distance > _maxDistance) {
    // _status = DISTANCE_SENSOR_ERROR;
    Serial.println("距离传感器测量错误");
    // return -1;
  }
  
  _status = DISTANCE_SENSOR_READY;
  return distance;
}

DistanceSensorStatus DistanceSensorManager::getStatus() const {
  return _status;
}

void DistanceSensorManager::setEnabled(bool enabled) {
  _isEnabled = enabled;
  if (enabled) {
    _status = DISTANCE_SENSOR_READY;
  } else {
    _status = DISTANCE_SENSOR_IDLE;
  }
}

bool DistanceSensorManager::isEnabled() const {
  return _isEnabled;
}

void DistanceSensorManager::setMeasureInterval(unsigned long interval) {
  _measureInterval = interval;
}

void DistanceSensorManager::setOnDistanceChanged(std::function<void(double)> callback) {
  _onDistanceChanged = callback;
}